{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 30,
   "id": "988b3172",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[-7.66044443e-01  6.42787610e-01  7.87187789e-17 -5.44000000e-01]\n",
      " [ 6.42787610e-01  7.66044443e-01  9.38133875e-17 -2.03000000e-01]\n",
      " [ 0.00000000e+00  1.22464680e-16 -1.00000000e+00 -3.70000000e-02]\n",
      " [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]\n",
      "[[ 0.70710678 -0.70710678  0.          0.056     ]\n",
      " [ 0.70710678  0.70710678  0.         -0.056     ]\n",
      " [ 0.          0.          1.         -0.106     ]\n",
      " [ 0.          0.          0.          1.        ]]\n",
      "[[ 0.70710678  0.70710678  0.          0.02      ]\n",
      " [-0.70710678  0.70710678  0.         -0.04      ]\n",
      " [ 0.          0.          1.          0.3       ]\n",
      " [ 0.          0.          0.          1.        ]]\n",
      "[[-8.71557427e-02  9.96194698e-01  7.87187789e-17 -6.22894595e-01]\n",
      " [ 9.96194698e-01  8.71557427e-02  9.38133875e-17 -2.09902383e-01]\n",
      " [ 8.65956056e-17  8.65956056e-17 -1.00000000e+00  6.90000000e-02]\n",
      " [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]\n",
      "[[-7.66044443e-01  6.42787610e-01  7.87187789e-17 -6.64485498e-01]\n",
      " [ 6.42787610e-01  7.66044443e-01  9.38133875e-17 -1.93464718e-01]\n",
      " [ 0.00000000e+00  1.22464680e-16 -1.00000000e+00 -2.31000000e-01]\n",
      " [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]\n"
     ]
    }
   ],
   "source": [
    "#!/usr/bin/env python\n",
    "# coding: utf-8\n",
    "import transforms3d as tfs\n",
    "import numpy as np\n",
    "import math\n",
    "\n",
    "def get_matrix_eular_radu(x,y,z,rx,ry,rz,rs=\"sxyz\"):\n",
    "    rmat = tfs.euler.euler2mat(math.radians(rx),math.radians(ry),math.radians(rz),rs)\n",
    "    rmat = tfs.affines.compose(np.squeeze(np.asarray((x,y,z))), rmat, [1, 1, 1])\n",
    "    return rmat\n",
    "\n",
    "\n",
    "\"\"\"\n",
    "   /* base@grapper  */\n",
    "    Matrix4d Tbg = TransForms::ComposeEuler(-0.544, -0.203,-0.037, 180, 0.00000, 140);\n",
    "    /* grapper@camera */\n",
    "    Matrix4d Tgc  = TransForms::ComposeEuler(0.056,-0.056,-0.106,0,-0,45);\n",
    "    /*  camera@maker*/\n",
    "    Matrix4d Tcw  = TransForms::ComposeEuler(0.056,-0.056,-0.106,0,-0,45);\n",
    "    Matrix4d Tbw  = TransForms::ComposeEuler(-0.663,-0.193,-0.231,-180,0,140);\n",
    "# Tcg = np.dot(np.linalg.pinv(MA),MB).reshape(3,)\n",
    "# print(tfs.affines.compose(Tcg,np.squeeze(Rcg),[1,1,1]))\n",
    "\"\"\"\n",
    "Tbg = get_matrix_eular_radu(-0.544, -0.203,-0.037, 180, 0.00000, 140)\n",
    "Tgc = get_matrix_eular_radu(0.056,-0.056,-0.106,0,0,45)\n",
    "Tcw = get_matrix_eular_radu(0.020,-0.040,0.300,0,0,-45)\n",
    "Tbc = np.dot(Tbg,Tgc)\n",
    "Tbw = np.dot(Tbc,Tcw)\n",
    "print(Tbg)\n",
    "print(Tgc)\n",
    "print(Tcw)\n",
    "print(Tbc)\n",
    "print(Tbw)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 36,
   "id": "ab747047",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 0.29159589,  0.3012824 , -0.90785503,  0.043     ],\n",
       "       [-0.92544348, -0.15118202, -0.3474167 , -0.163     ],\n",
       "       [-0.2419219 ,  0.9414738 ,  0.23473578,  0.0571    ],\n",
       "       [ 0.        ,  0.        ,  0.        ,  1.        ]])"
      ]
     },
     "execution_count": 36,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "import transforms3d as tfs\n",
    "import numpy as np\n",
    "import math\n",
    "def get_matrix_eular_radu(x,y,z,rx,ry,rz,rs=\"sxyz\"):\n",
    "    rmat = tfs.euler.euler2mat(math.radians(rx),math.radians(ry),math.radians(rz),rs)\n",
    "    rmat = tfs.affines.compose(np.squeeze(np.asarray((x,y,z))), rmat, [1, 1, 1])\n",
    "    return rmat\n",
    "rbl = get_matrix_eular_radu(0.043, -0.163, 0.0571,  76,14,-72.511)\n",
    "rlp = get_matrix_eular_radu(0.300746, 0.096994,0 ,0 ,0 ,0 )\n",
    "rbl"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "id": "05547d02",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([ 0.58633387,  0.54955485, -0.28393939, -0.52304921])"
      ]
     },
     "execution_count": 27,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "np.dot(rbl,rlp)\n",
    "tfs.euler.mat2euler(np.dot(rbl,rlp)[0:3,0:3])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 37,
   "id": "ef5b0cc9",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[-0.13233733, -0.75017583, -0.64786037],\n",
       "       [ 0.03440783, -0.65669189,  0.75337366],\n",
       "       [-0.99060736,  0.07740798,  0.11271677]])"
      ]
     },
     "execution_count": 37,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "a = np.asarray([0.300746, 0.096994,0])\n",
    "tfs.euler.euler2mat(76, 14, -72.511)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 50,
   "id": "3623a738",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 0.29159589,  0.3012824 , -0.90785503],\n",
       "       [-0.92544348, -0.15118202, -0.3474167 ],\n",
       "       [-0.2419219 ,  0.9414738 ,  0.23473578]])"
      ]
     },
     "execution_count": 50,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "rbl = get_matrix_eular_radu(0.043, -0.163, 0.0571,  76,14,-72.511)\n",
    "el = rbl[0:3,0:3]\n",
    "el"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 52,
   "id": "28144bef",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([-72.51100124,  14.00000024,  76.0000013 ])"
      ]
     },
     "execution_count": 52,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "np.asarray(tfs.euler.mat2euler(el,\"rzyx\"))*180/3.1415926"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 53,
   "id": "07a31b99",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([ 76.0000013 ,  14.00000024, -72.51100124])"
      ]
     },
     "execution_count": 53,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "np.asarray(tfs.euler.mat2euler(el,\"sxyz\"))*180/3.1415926"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4a558599",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
